from threading import Condition
import time
import typing
import json
import fastdds
import CommonData

class MyWriterListener(fastdds.DataWriterListener):
    def __init__(self):
        self._matched_reader = 0
        super().__init__()
        
    def on_liveliness_lost(self, writer:fastdds.DataWriter, status:fastdds.PublicationMatchedStatus):
        return super().on_liveliness_lost(writer, status)
    
    def on_offered_deadline_missed(self, writer: fastdds.DataWriter, status: fastdds.OfferedDeadlineMissedStatus):
        return super().on_offered_deadline_missed(writer, status)
    
    def on_offered_incompatible_qos(self, writer: fastdds.DataWriter, status: fastdds.OfferedIncompatibleQosStatus):
        return super().on_offered_incompatible_qos(writer, status)
    
    def on_publication_matched(self, writer:fastdds.DataWriter, info:fastdds.LivelinessLostStatus):
        return super().on_publication_matched(writer, info)
    
    def on_unacknowledged_sample_removed(self, writer: fastdds.DataWriter, instance: fastdds.InstanceHandle_t):
        return super().on_unacknowledged_sample_removed(writer, instance)

class MyReaderListener(fastdds.DataReaderListener):
    def __init__(
        self, 
        cb_on_data_available:typing.Callable[[str, dict], None]):
        super().__init__()
        self._cb_on_data_available = cb_on_data_available
        
    def on_data_available(self, reader:fastdds.DataReader):
        super().on_data_available(reader)
        info = fastdds.SampleInfo()
        data = CommonData.CommonData()
        reader.take_next_sample(data, info)
        
        if (not info.valid_data):
            print("error recv")
            return
        d = json.loads(data.data())
        topic_description: fastdds.TopicDescription = reader.get_topicdescription()
        self._cb_on_data_available(topic_description.get_name(), d)
    
    def on_liveliness_changed(self, reader:fastdds.DataReader, status:fastdds.SubscriptionMatchedStatus):
        return super().on_liveliness_changed(reader, status)
    
    def on_requested_deadline_missed(self, reader:fastdds.DataReader, status:fastdds.RequestedDeadlineMissedStatus):
        return super().on_requested_deadline_missed(reader, status)
    
    def on_requested_incompatible_qos(self, reader:fastdds.DataReader, status:fastdds.RequestedIncompatibleQosStatus):
        return super().on_requested_incompatible_qos(reader, status)
    
    def on_sample_lost(self, reader:fastdds.DataReader, status:fastdds.SampleLostStatus):
        return super().on_sample_lost(reader, status)
    
    def on_sample_rejected(self, reader:fastdds.DataReader, status:fastdds.SampleRejectedStatus):
        return super().on_sample_rejected(reader, status)
    
    def on_subscription_matched(self, reader:fastdds.DataReader, info:fastdds.SubscriptionMatchedStatus):
        return super().on_subscription_matched(reader, info)


class DdsClient(object):
    def __init__(self, node_name:str, domain_id:int=100) -> None:
        self.node_name = node_name
        self.domain_id = domain_id
        
        # qos
        self._participant_qos = fastdds.DomainParticipantQos()
        self._participant_qos.name(self.node_name)
        
        # data_type
        self._topic_data_type = CommonData.CommonDataPubSubType()
        self._topic_data_type.setName("CommonData")
        self._type_support = fastdds.TypeSupport(self._topic_data_type)
        
        # 创建 participant
        factory: fastdds.DomainParticipantFactory = fastdds.DomainParticipantFactory.get_instance()
        factory.get_default_participant_qos(self._participant_qos)
        self._participant: fastdds.DomainParticipant = factory.create_participant(self.domain_id, self._participant_qos)
        self._participant.register_type(self._type_support)
        
        # 创建 publiser_qos
        self._publishe_qos = fastdds.PublisherQos()
        self._participant.get_default_publisher_qos(self._publishe_qos)
        
        # 创建 subscriber_qos
        self._subscriber_qos = fastdds.SubscriberQos()
        self._participant.get_default_subscriber_qos(self._subscriber_qos)
        
        # 创建 topic_qos
        self._topic_qos = fastdds.TopicQos()
        self._participant.get_default_topic_qos(self._topic_qos)
        
        # 管理 dict
        self._publish_topic_map_publisher_datawriter_topic: typing.Dict[str, typing.Tuple[fastdds.Publisher, fastdds.DataWriter, fastdds.Topic]] = dict()
        self._subscribe_topic_map_subscriber_datareader_topic: typing.Dict[str, typing.Tuple[fastdds.Subscriber, fastdds.DataReader, fastdds.Topic]] = dict()
    
    def publish(self, topic:str, data:dict) -> None:
        if (topic not in self._publish_topic_map_publisher_datawriter_topic.keys()):
            self._create_publisher(topic)
        common_data = CommonData.CommonData()
        common_data.data(json.dumps(data))
        publisher = self._publish_topic_map_publisher_datawriter_topic[topic][1].write(common_data)
    
    def subscribe(self, topic:str, cb_on_data_available:typing.Callable[[str, dict], None]) -> None:
        '''
        订阅一个消息，设置数据接收的回调函数
        '''
        if (topic not in self._subscribe_topic_map_subscriber_datareader_topic.keys()):
            self._create_subscriber(topic, cb_on_data_available)
        
    def _create_publisher(self, topic:str) -> None:
        '''
        创建一个 publisher
        '''
        # 1. 创建 topic
        topic_dds: fastdds.Topic = self._participant.create_topic(topic, self._topic_data_type.getName(), self._topic_qos)
        assert (isinstance(topic_dds, fastdds.Topic)), ValueError("创建 publisher topic: {0} error".format(topic))
        
        # 2. 创建 publisher
        publisher: fastdds.Publisher = self._participant.create_publisher(self._publishe_qos)
        assert (isinstance(topic_dds, fastdds.Topic)), ValueError("创建 publisher: {0} error".format(topic))
         
        # 3. 创建 writer
        listener = MyWriterListener()
        writer_qos = fastdds.DataWriterQos()
        publisher.get_default_datawriter_qos(writer_qos)
        writer: fastdds.DataWriter = publisher.create_datawriter(topic_dds, writer_qos, listener)
        assert (isinstance(writer, fastdds.DataWriter)), ValueError("创建 publisher writer: {0} error".format(topic))
        
        # 4. 写入 dict
        self._publish_topic_map_publisher_datawriter_topic[topic] = (publisher, writer, topic_dds)
    
    def _create_subscriber(self, topic:str,  cb_on_data_available:typing.Callable[[str, dict], None]) -> None:
        # 1. 创建 topic
        topic_dds: fastdds.Topic = self._participant.create_topic(topic, self._topic_data_type.getName(), self._topic_qos)
        assert (isinstance(topic_dds, fastdds.Topic)), ValueError("创建 subscribe topic: {0} error".format(topic))
        
        # 2. 创建 subscriber
        subscriber: fastdds.Subscriber = self._participant.create_subscriber(self._subscriber_qos)
        assert (isinstance(subscriber, fastdds.Subscriber)), ValueError("创建 subscriber: {0} error".format(topic))
        
        # 3. 创建 listener
        listener = MyReaderListener(cb_on_data_available)
        listener_qos = fastdds.DataReaderQos()
        subscriber.get_default_datareader_qos(listener_qos)
        reader: fastdds.DataReader = subscriber.create_datareader(topic_dds, listener_qos, listener)
        assert (isinstance(reader, fastdds.DataReader)), ValueError("创建 subscriber reader: {0} error".format(topic))
        
        # 4. 写入 dict
        self._subscribe_topic_map_subscriber_datareader_topic[topic] = (subscriber, reader, topic_dds)
        
    def __del__(self):
        factory = fastdds.DomainParticipantFactory.get_instance()
        self._participant.delete_contained_entities()
        factory.delete_participant(self._participant)
        print("into DdsClient __del__ ")
